#include "Translate.h"
#include <string.h>

void Translate::init(Dof *x, Dof *y, Dof *z, char *_name){
	mDofs.resize(3);
	mDofs[0]=x;
	mDofs[1]=y;
	mDofs[2]=z;
	mDofs[0]->setTrans(this);
	mDofs[1]->setTrans(this);
	mDofs[2]->setTrans(this);
	strcpy(mDofs[0]->getName(), "_transX");
	strcpy(mDofs[1]->getName(), "_transY");
	strcpy(mDofs[2]->getName(), "_transZ");
	mType = Transformation::T_TRANSLATE;
	strcpy(mName, _name);
}

Translate::Translate(Dof *x, Dof *y, Dof *z){
	init(x,y,z, "Translate");
}

Translate::Translate(Dof *x, Dof *y, Dof *z, char *_name){
	init(x,y,z,_name);
}

Mat4d *Translate::getTransformMat(){
	(*mTransform)[0][0] = 1.0; 
	(*mTransform)[1][1] = 1.0; 
	(*mTransform)[2][2] = 1.0; 
	(*mTransform)[3][3] = 1.0; 
	for(int i=0; i<mDofs.size(); i++){
		(*mTransform)[i][3] = mDofs[i]->getValue();
	}
	return mTransform;
}

Mat4d *Translate::getDerivativeMat(Dof *d){
	int i=-1;
	if(mDofs[0]->getID()==d->getID()) i=0;
	else if(mDofs[1]->getID()==d->getID()) i=1;
	else if(mDofs[2]->getID()==d->getID()) i=2;
	(*mDeriv) = vl_0;
	if(i!=-1) (*mDeriv)[i][3] = 1.0; 
	return mDeriv;
}

Mat4d *Translate::getDerivativeMat(Dof *d1, Dof *d2){
	// it is 0 for any 2 mDofs
	(*mDeriv) = vl_0;
	return mDeriv;
}

void Translate::applyGLTransform(){
	glTranslatef(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());
}

void Translate::applyTransformMat(Vec4d *v){
	for(int i=0; i<mDofs.size(); i++) (*v)[i]+=(*v)[3]*mDofs[i]->getValue();
}

void Translate::applyTransformMat(Mat4d *m){
	for(int i=0; i<mDofs.size(); i++) {
		double val = mDofs[i]->getValue();
		for(int j=0; j<4; j++) {
			(*m)[i][j]+=val*(*m)[3][j];
		}
	}
}

void Translate::applyDerivativeMat(Dof *d, Vec4d *v){
	int i=-1;
	if(mDofs[0]->getID()==d->getID()) i=0;
	else if(mDofs[1]->getID()==d->getID()) i=1;
	else if(mDofs[2]->getID()==d->getID()) i=2;
	assert(i!=-1);
	double val = (*v)[3];
	(*v) = vl_0;
	(*v)[i]=val;
}

void Translate::applyDerivativeMat(Dof *d, Mat4d *m){
	int i=-1;
	if(mDofs[0]->getID()==d->getID()) i=0;
	else if(mDofs[1]->getID()==d->getID()) i=1;
	else if(mDofs[2]->getID()==d->getID()) i=2;
	assert(i!=-1);
	Vec4d v = (*m)[3];
	(*m) = vl_0;
	for(int j=0; j<4; j++) {
		(*m)[i][j]=v[j];
	}
}

void Translate::applyDerivativeMat(Dof *d1, Dof *d2, Vec4d *v){
	(*v)=vl_0;
}

void Translate::applyDerivativeMat(Dof *d1, Dof *d2, Mat4d *v){
	(*v)=vl_0;
}

void TranslateX::init(Dof *x, char *_name){
	mDofs.resize(1);
	mDofs[0]=x;
	mDofs[0]->setTrans(this);
	strcpy(mDofs[0]->getName(), "_transX");
	mType = Transformation::T_TRANSLATEX;
	strcpy(mName, _name);
	mTransform = new Mat4d;
	*mTransform = vl_0;
	mDeriv = new Mat4d;
	*mDeriv= vl_0;
}
TranslateX::TranslateX(Dof *x){
	init(x, "TranslateX");
}

TranslateX::TranslateX(Dof *x, char *_name){
	init(x, _name);
}

Mat4d *TranslateX::getTransformMat(){
	(*mTransform)[0][0] = 1.0; 
	(*mTransform)[1][1] = 1.0; 
	(*mTransform)[2][2] = 1.0; 
	(*mTransform)[3][3] = 1.0; 
	(*mTransform)[0][3] = mDofs[0]->getValue(); 
	return mTransform;
}

Mat4d *TranslateX::getDerivativeMat(Dof *d){
	int i=-1;
	if(mDofs[0]->getID()==d->getID()) i=0;
	(*mDeriv) = vl_0;
	if(i!=-1) (*mDeriv)[i][3] = 1.0; 
	return mDeriv;
}

Mat4d *TranslateX::getDerivativeMat(Dof *d1, Dof *d2){
	// it is 0 and d1 has to be =d2
	(*mDeriv) = vl_0;
	return mDeriv;
}

void TranslateX::applyGLTransform(){
	glTranslatef(mDofs[0]->getValue(), 0, 0);
}

void TranslateX::applyTransformMat(Vec4d *v){
	(*v)[0]+=(*v)[3]*mDofs[0]->getValue();
}

void TranslateX::applyTransformMat(Mat4d *m){
	double val = mDofs[0]->getValue();
	for(int i=0; i<4; i++) {
		(*m)[0][i]+=val*(*m)[3][i];
	}
}

void TranslateX::applyDerivativeMat(Dof *d, Vec4d *v){
	(*v)[0]=(*v)[3];
	(*v)[1]=0;
	(*v)[2]=0;
	(*v)[3]=0;
}

void TranslateX::applyDerivativeMat(Dof *d1, Dof *d2, Vec4d *v){
	(*v)=vl_0;
}

void TranslateX::applyDerivativeMat(Dof *d, Mat4d *m){
	Vec4d v = (*m)[3];
	(*m)=vl_0;
	for(int i=0; i<4; i++) {
		(*m)[0][i]=v[i];
	}
}

void TranslateX::applyDerivativeMat(Dof *d1, Dof *d2, Mat4d *v){
	(*v)=vl_0;
}

// Translate along y axis

void TranslateY::init(Dof *x, char *_name){
	mDofs.resize(1);
	mDofs[0]=x;
	mDofs[0]->setTrans(this);
	strcpy(mDofs[0]->getName(), "_transY");
	mType = Transformation::T_TRANSLATEY;
	strcpy(mName, _name);
	mTransform = new Mat4d;
	*mTransform = vl_0;
	mDeriv = new Mat4d;
	*mDeriv= vl_0;
}
TranslateY::TranslateY(Dof *x){
	init(x, "TranslateY");
}

TranslateY::TranslateY(Dof *x, char *_name){
	init(x, _name);
}

Mat4d *TranslateY::getTransformMat(){
	(*mTransform)[0][0] = 1.0; 
	(*mTransform)[1][1] = 1.0; 
	(*mTransform)[2][2] = 1.0; 
	(*mTransform)[3][3] = 1.0; 
	(*mTransform)[1][3] = mDofs[0]->getValue(); 
	return mTransform;
}

Mat4d *TranslateY::getDerivativeMat(Dof *d){
	int i=-1;
	if(mDofs[0]->getID()==d->getID()) i=1;
	(*mDeriv) = vl_0;
	if(i!=-1) (*mDeriv)[i][3] = 1.0; 
	return mDeriv;
}

Mat4d *TranslateY::getDerivativeMat(Dof *d1, Dof *d2){
	// it is 0 and d1 has to be =d2
	(*mDeriv) = vl_0;
	return mDeriv;
}

void TranslateY::applyGLTransform(){
	glTranslatef(0, mDofs[0]->getValue(), 0);
}

void TranslateY::applyTransformMat(Vec4d *v){
	(*v)[1]+=(*v)[3]*mDofs[0]->getValue();
}

void TranslateY::applyTransformMat(Mat4d *m){
	double val = mDofs[0]->getValue();
	for(int i=0; i<4; i++) {
		(*m)[1][i]+=val*(*m)[3][i];
	}
}

void TranslateY::applyDerivativeMat(Dof *d, Vec4d *v){
	(*v)[0]=0;
	(*v)[1]=(*v)[3];
	(*v)[2]=0;
	(*v)[3]=0;
}

void TranslateY::applyDerivativeMat(Dof *d, Mat4d *m){
	Vec4d v = (*m)[3];
	*m=vl_0;
	for(int i=0; i<4; i++) {
		(*m)[1][i]=v[i];
	}
}

void TranslateY::applyDerivativeMat(Dof *d1, Dof *d2, Vec4d *v){
	(*v)=vl_0;
}

void TranslateY::applyDerivativeMat(Dof *d1, Dof *d2, Mat4d *v){
	(*v)=vl_0;
}

// translate along z
void TranslateZ::init(Dof *x, char *_name){
	mDofs.resize(1);
	mDofs[0]=x;
	mDofs[0]->setTrans(this);
	strcpy(mDofs[0]->getName(), "_transZ");
	mType = Transformation::T_TRANSLATEZ;
	strcpy(mName, _name);
	mTransform = new Mat4d;
	*mTransform = vl_0;
	mDeriv = new Mat4d;
	*mDeriv= vl_0;
}

TranslateZ::TranslateZ(Dof *x){
	init(x, "TranslateZ");
}

TranslateZ::TranslateZ(Dof *x, char *_name){
	init(x, _name);
}

Mat4d *TranslateZ::getTransformMat(){
	(*mTransform)[0][0] = 1.0; 
	(*mTransform)[1][1] = 1.0; 
	(*mTransform)[2][2] = 1.0; 
	(*mTransform)[3][3] = 1.0; 
	(*mTransform)[2][3] = mDofs[0]->getValue(); 
	return mTransform;
}

Mat4d *TranslateZ::getDerivativeMat(Dof *d){
	int i=-1;
	if(mDofs[0]->getID()==d->getID()) i=2;
	(*mDeriv) = vl_0;
	if(i!=-1) (*mDeriv)[i][3] = 1.0; 
	return mDeriv;
}

Mat4d *TranslateZ::getDerivativeMat(Dof *d1, Dof *d2){
	// it is 0 and d1 has to be =d2
	(*mDeriv) = vl_0;
	return mDeriv;
}

void TranslateZ::applyGLTransform(){
	glTranslatef(0, 0, mDofs[0]->getValue());
}

void TranslateZ::applyTransformMat(Vec4d *v){
	(*v)[2]+=(*v)[3]*mDofs[0]->getValue();
}

void TranslateZ::applyTransformMat(Mat4d *m){
	double val = mDofs[0]->getValue();
	for(int i=0; i<4; i++) {
		(*m)[2][i]+=val*(*m)[3][i];
	}
}

void TranslateZ::applyDerivativeMat(Dof *d, Vec4d *v){
	(*v)[0]=0;
	(*v)[1]=0;
	(*v)[2]=(*v)[3];
	(*v)[3]=0;
}

void TranslateZ::applyDerivativeMat(Dof *d, Mat4d *m){
	Vec4d v = (*m)[3];
	*m=vl_0;
	for(int i=0; i<4; i++) {
		(*m)[2][i]=v[i];
	}
}

void TranslateZ::applyDerivativeMat(Dof *d1, Dof *d2, Vec4d *v){
	(*v)=vl_0;
}

void TranslateZ::applyDerivativeMat(Dof *d1, Dof *d2, Mat4d *v){
	(*v)=vl_0;
}
